function [ dfB ] = accerror_simp( DupsilonBl, dfBiasS, dKScalP1, dPSS0, dKScalP2, ignore2Od ) %#codegen
%ACCERROR_SIMP 计算加速度计器件误差引入的测量误差
%
% Tips:
% 1. 对增量计算，传入的dfBiasS需要乘以采样周期，传入的dKScalP2需要除以采样周期
%
% Input Arguments:
% DupsilonBl: m*3矩阵，m为采样个数，比力，单位m/s^2
% dfBiasS: m*3矩阵，零偏，单位m/s^2，默认全为0
% dKScalP1: m*3矩阵，标度因数误差，对应dKScalP第一列，无单位，默认全为0
% dPSS0: 3*3*m矩阵，失准角（安装误差），无单位，默认全为0
% dKScalP2: m*3矩阵，二次项系数，对应dKScalP第二列，单位1/(m/s^2)，默认全为0
% ignore2Od: 逻辑标量，true表示忽略2阶误差，默认为true
%
% Output Arguments:
% dfB: m*3矩阵，各采样的比力误差，单位m/s^2
%
% References:
% [1] 理论文档“惯组误差模型”第2节

m = size(DupsilonBl, 1);
if nargin < 2
    dfBiasS = zeros(m, 3);
end
if nargin < 3
    dKScalP1 = zeros(m, 3);
end
if nargin < 4
    dPSS0 = zeros(3, 3, m);
end
if nargin < 5
    dKScalP2 = zeros(m, 3);
end
if nargin < 6
    ignore2Od = true;
end

dSFMAErr = coder.nullcopy(NaN(m, 3));
for i=1:m
    dSFMAErr(i, :) = DupsilonBl(i, :) * (diag(dKScalP1(i, :))+dPSS0(:, :, i)')';% MA
end
dfB1 = dfBiasS + dSFMAErr + dKScalP2.*(DupsilonBl.^2);

if ignore2Od
    dfB2 = zeros(m, 3);
else
    for i=1:m
        dSFMAErr(i, :) = DupsilonBl(i, :)*dPSS0(:, :, i); % MA'
    end
    dfB2 = dKScalP1.*(dfBiasS + dSFMAErr + dKScalP2.*(DupsilonBl.^2));
end
dfB = dfB1 + dfB2;
end